#include "vio/vio_pipeline.h"

namespace vio {

void VIOPipeline::Initialize() {
  // init left camera
  Eigen::Matrix3d left_cam_in_K;
  Eigen::Matrix3d left_cam_out_K;
  Eigen::Vector4d left_cam_D;
  left_cam_in_K << 367.4688907375704, 0.0, 475.1072540283203, 0.0,
      367.4688907375704, 213.98190307617188, 0.0, 0.0, 1.0;
  left_cam_out_K << 367.2334931786206, 0.0, 389.39469245969235, 0.0,
      367.3189237170502, 238.26256236912505, 0.0, 0.0, 1.0;
  // kannala_brandt or equidistant distortion models
  left_cam_D << -0.028610502986514477, 0.026582562014177236,
      -0.044875987453798845, 0.021906788800146022;
  int width = 752;
  int height = 480;
  camera_ptr_ = std::make_shared<Camera>(
      width, height, left_cam_in_K, left_cam_out_K, left_cam_D);
  fast_detector_ptr_ =
      std::make_shared<feature_detection::FastDetector>(width, height, 30, 3);
}

void VIOPipeline::Step() {}

// msg callbacks
void VIOPipeline::IMUCallback(const double& ts,
                              const Eigen::Vector3d& accel,
                              const Eigen::Vector3d& gyro) {}

void VIOPipeline::LeftImgCallback(const double& ts, const cv::Mat& img) {
  cv::Mat rect_img = cv::Mat(img.rows, img.cols, CV_8UC1);
  camera_ptr_->undistortImage(img, rect_img);
  curr_frame_ptr_ = std::make_shared<Frame>(camera_ptr_, rect_img, ts);
  fast_detector_ptr_->Detect(curr_frame_ptr_,
                             curr_frame_ptr_->img_pyr_,
                             10,
                             curr_frame_ptr_->features_);
  LOG(INFO) << "features: " << curr_frame_ptr_->features_.size();
  DrawKeyPoints(rect_img, curr_frame_ptr_->features_);
}

void VIOPipeline::RightImgCallback(const double& ts, const cv::Mat& img) {}

void VIOPipeline::Terminate() {}

// cv tools
void VIOPipeline::DrawKeyPoints(cv::Mat& img, vio::Features& fts) {
  cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
  cv::cvtColor(img, img_rgb, cv::COLOR_GRAY2RGB);
  std::for_each(fts.begin(), fts.end(), [&](vio::Feature::Ptr fi) {
    cv::circle(img_rgb,
               cv::Point2f(fi->px[0], fi->px[1]),
               1 * (fi->level + 1),
               cv::Scalar(0, 255, 0),
               1);
  });
  cv::imshow("feature", img_rgb);
  cv::waitKey(1);
}

}  // namespace vio
